#include "Evaluation.h"
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace mrpt::slam;
using namespace std;

double EvaluationTraceCov::evaluate(Robot& robot, CRangeBearingKFSLAM2DWithCopy &mapping, const mrpt::slam::CLandmarksMap& landmarkMap, const RangeSensorModel& sensor) {
    CActionCollectionPtr actions;
    CSensoryFramePtr observations;
    //printf("Evaluation Start\n");
    CMatrixDouble fullCov;
    CVectorDouble fullState;
    CPosePDFGaussian robotPose;
    std::vector<TPoint2D> LMs;
    std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs;
    //Sense
    
    robot.setSensorParameters(sensor);
    //printf("Robot Start\n");
    robot.sense(actions, observations, landmarkMap);
    //Execute SLAM
    
    //printf("Robot End\n");
    mapping.processActionObservation(actions, observations);
    mapping.getCurrentState(robotPose, LMs, LM_IDs, fullState, fullCov);
    
    //printf("Mapping init\n");
    //Eigen::Matrix<double,Dynamic,Dynamic> cov(fullCov);
    actions.clear_unique();
    observations.clear_unique();
    double trace = fullCov.determinant();
    //std::cout << "TRACE: " << trace << std::endl;
    return trace;
}
